#define _GNU_SOURCE 1
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <getopt.h>

#include <stdio.h>
// #include <stdlib.h> // 为了使用atoi函数
// #include <string.h>

#include "ukfCfg20.h"

#define cfg0 (int)0
// #define cfg1 (int)1

/*---------------------------------------------*/
/*           Function Prototype                */
/*---------------------------------------------*/
int ukf_test(void);
int verbose = 0;
void show_matrix_obj(Matrix_t A);
void show_matrix(double *A, int n, int m);
int main()
{
	
	int error = 0;
	error |= ukf_test();
	return 0;
}
#define MAX_LEN 80000 // 定义一行文本最大长度
int ukf_test(void)
{
	_Bool tfInitCfg0 = 0;
	UKF_t ukfIo[2];
	int simLoop;

	// UKF滤波器测量输入（数据日志在matlab中生成，用于UKF仿真15次迭代）
	FILE* fp;
    char line[MAX_LEN];
    char* token; // 用于存储每个逗号分隔的值
    // int value;
    int CNT=0;

    fp = fopen("data5_Gain200_press_cut.txt", "r"); // 打开文本文件
    if (fp == NULL) {
        printf("无法打开文件！\n");
        return -1;
    }

    fgets(line, MAX_LEN, fp); // 读取一行文本
    fclose(fp); // 关闭文件

    int data_arr[10000];

    token = strtok(line, ","); // 使用strtok函数获取每个逗号分隔的值
    while(token != NULL) {
        data_arr[CNT] = atoi(token); // 将字符串转换为整数类型
        CNT++;
        token = strtok(NULL, ","); // 继续查找下一个逗号分隔的值
    }
    printf("data_arr:%d\n", CNT);

    int row = CNT-1;
    double yt[10000];
    for(int i=0; i<row; i++) {
        yt[i] = (double)data_arr[i];
    }
	
	// UKF initialization: CFG0
	tfInitCfg0 = ukf_init(&ukfIo[cfg0], &UkfMatrixCfg20);
	// printf("x.nelem:%d,nrow:%d,ncol:%d.\n", ukfIo[cfg0].par.x0.nelem, ukfIo[cfg0].par.x0.nrow, ukfIo[cfg0].par.x0.ncol);
	
	printf("x0:%d\n",ukfIo[cfg0].par.x0.nelem);
	ukfIo[cfg0].par.x0.val[0] = yt[0];
	// ukfIo[cfg0].par.x0.val[1] = yt[1];
	// pUkf->prev.x_p.val
	printf("x_p:%d\n",ukfIo[cfg0].prev.x_p.nelem);
	ukfIo[cfg0].prev.x_p.val[0]=yt[0];
	printf("X_p:%d\n",ukfIo[cfg0].prev.X_p.nelem);
	// ukfIo[cfg0].prev.X_p.val[0]=yt[0];
	// ukfIo[cfg0].prev.X_p.val[1]=yt[0];
	// ukfIo[cfg0].prev.X_p.val[2]=yt[0];


	if (tfInitCfg0 == 0)
	{
		// UKF simulation CFG0: BEGIN
		for (simLoop = 1; simLoop < row; simLoop++)
		{
			double *const py_cfg0 = ukfIo[cfg0].input.y.val;
			double * const pu = ukfIo[cfg0].input.u.val;
			// UKF:CFG0 apply/load system measurements in working array for current iteration.
			py_cfg0[0] = yt[simLoop];
			//load system inputs if exist for current iteration
			pu[0] = yt[simLoop];//put your first system input
			
			// UKF:CFG0 periodic task call
			(void)ukf_step(&ukfIo[cfg0]);
 			printf("%2.14f,\n", 	ukfIo[cfg0].update.x.val[0]);
	
		}
		// UKF simulation CFG0: END
	}
	else
	{
		// initialization fail
		printf("initialization fail:%d.\n",tfInitCfg0);
	}
	return 0;
}


void show_matrix_obj(Matrix_t A)
{
	int i, j;

	for (i = 0; i < A.nrow; i++)
	{
		for (j = 0; j < A.ncol; j++)
		{
			printf("%2.14f ", A.val[A.ncol * i + j]);
		}
		printf("\n");
	}
	printf("\n");
}

void show_matrix(double *A, int n, int m)
{
	int i, j;

	for (i = 0; i < n; i++)
	{
		for (j = 0; j < m; j++)
		{
			printf("%2.14f ", A[m * i + j]);
		}
		printf("\n");
	}
	printf("\n");
}